#include "Physics.h"

#include "GameObject.h"
#include "GamePlayState.h"
#include "../SGD Wrappers/SGD_Math.h"

#pragma region Physics Methods

float Physics::Gravity= 190.0f;

// Constructor
Physics::Physics(void)
{
}

// Destructor
Physics::~Physics(void)
{
}

// BoxToBox
//	- Performs a collision check between two boxes.
//	- After the collision check is performed, a collisionInfo
//	  structure is filled with information about the collision.
//	- rect1 : the box with the focus of the collision.
//	- rect2 : the box to check collision against.
//	- RETURN : TRUE if a collision was detected. FALSE otherwise.
bool Physics::BoxToBox(CollisionInfo &collisionInfo, RECT rect1, RECT rect2)
{
	//Reset collision Info
	//Rect 1 is the main obj
	//Rect 2 is the one we are checking against
	//We are returning the side of rect2 that we coll on


	//Bool to determine if the rects are intersecting
	bool Intersecting = false;

	//Width and Height
	int Rec1Width = rect1.right - rect1.left;
	int Rec2Width = rect2.right - rect2.left;

	int Rec1Height = rect1.bottom - rect1.top;
	int Rec2Height = rect2.bottom - rect2.top;

	//Centers of each rect
	tVector2D Rect1Center;
	tVector2D Rect2Center;

	//Calc Center points
	Rect1Center.fX = (float)rect1.left + (Rec1Width / 2);
	Rect1Center.fY = (float)rect1.top + (Rec1Height / 2);

	Rect2Center.fX = (float)rect2.left + (Rec2Width / 2);
	Rect2Center.fY = (float)rect2.top + (Rec2Height / 2);

	//Check to see if objects are colliding
	Intersecting = ((rect1.left < rect2.right) && (rect1.right > rect2.left) && (rect1.top < rect2.bottom) && (rect1.bottom > rect2.top));

	//Collision Info - Side that collided
	if( Intersecting == true)
	{
		RECT Overlapping;
		IntersectRect(&Overlapping, &rect1, &rect2);

		////Get the upper right of the overlapping rect
		//if( rect1.top > rect2.top )
		//	Overlapping.top = rect1.top;
		//else
		//	Overlapping.top = rect2.top;

		//if( rect1.left > rect2.left )
		//	Overlapping.left = rect1.left;
		//else
		//	Overlapping.left = rect2.left;

		////Vars for the width and height of the overlapping rect
		//int width = 0;
		//int height = 0;

		////Calc the width and height
		//if( rect1.left <= rect2.left) 
		//	width = rect1.right - rect2.left;
		//else if( rect1.left > rect2.left )
		//	width = rect2.right - rect1.left;
		//else
		//{
		//	width = ((rect2.right - rect2.left) > (rect1.right - rect1.left)) ? (rect1.right - rect1.left) : (rect2.right - rect2.left);
		//}

		//if( rect1.top <= rect2.top)
		//	height = rect1.bottom - rect2.top;
		//else if( rect1.top > rect2.top )
		//	height = rect2.bottom - rect1.top;
		//else
		//	height = ((rect2.bottom - rect2.top) > (rect1.bottom - rect1.top)) ? (rect1.bottom - rect1.top) : (rect2.bottom - rect2.top);

		////Calc the right and bottom based on the width and height
		//Overlapping.right = Overlapping.left + width;
		//Overlapping.bottom = Overlapping.top + height;


		//Set the Collinfo Overlap
		collisionInfo.Overlap = Overlapping;


		//Calculating what side of collision
		int OverLapWidth = Overlapping.right - Overlapping.left;
		int OverLapHeight = Overlapping.bottom - Overlapping.top;



		if( OverLapWidth > OverLapHeight )
		{
			//Vertical Collision
			if( Rect1Center.fY < Rect2Center.fY )
			{
				collisionInfo.Side = TOP;
			}
			else
			{
				collisionInfo.Side = BOTTOM;
			}
		}
		else
		{
			//Horizontal Collision
			if( Rect1Center.fX < Rect2Center.fX )
			{
				collisionInfo.Side = LEFT;
			}
			else
			{
				collisionInfo.Side = RIGHT;
			}
		}

		//Check to see if there is a perfect square
		if( OverLapWidth == OverLapHeight )
		{
			//Horizontal Collision
			if( Rect1Center.fX < Rect2Center.fX )
			{
				collisionInfo.Side = LEFT;
			}
			else
			{
				collisionInfo.Side = RIGHT;
			}
		}

		//	//CornerPoints
		//	tVector2D UpperLeftCorner;
		//	tVector2D UpperRightCorner;
		//	tVector2D BottomLeftCorner;
		//	tVector2D BottomRightCorner;

		//	//Get Corners
		//	UpperLeftCorner.fX = (float)rect1.left;
		//	UpperLeftCorner.fY = (float)rect1.top;

		//	UpperRightCorner.fX = (float)rect1.right;
		//	UpperRightCorner.fY = (float)rect1.top;

		//	BottomLeftCorner.fX = (float)rect1.left;
		//	BottomLeftCorner.fY = (float)rect1.bottom;

		//	BottomRightCorner.fX = (float)rect1.right;
		//	BottomRightCorner.fY = (float)rect1.bottom;

		//	//OverLappingCenter
		//	tVector2D OverlappingCenter;
		//	OverlappingCenter.fX = (float)Overlapping.left + OverLapWidth / 2;
		//	OverlappingCenter.fY = (float)Overlapping.top + OverLapHeight / 2;

		//	tVector2D LeftSide = LineCollision( Rect2Center, OverlappingCenter, UpperLeftCorner, BottomLeftCorner, nullptr);
		//	tVector2D RightSide = LineCollision( Rect2Center, OverlappingCenter, UpperRightCorner, BottomRightCorner, nullptr);
		//	tVector2D TopSide = LineCollision( Rect2Center, OverlappingCenter, UpperLeftCorner, UpperRightCorner, nullptr);
		//	tVector2D BottomSide = LineCollision( Rect2Center, OverlappingCenter, BottomLeftCorner, BottomRightCorner, nullptr);

		//	//Collides Left Side
		//	if( LeftSide.fX != 0 && LeftSide.fY != 0)
		//	{
		//		collisionInfo.Side = RIGHT;
		//	}

		//	//Collides Right Side
		//	if( RightSide.fX != 0 && RightSide.fY != 0)
		//	{
		//		collisionInfo.Side = LEFT;
		//	}

		//	//Collides Bottom Side
		//	if( BottomSide.fX != 0 && BottomSide.fY != 0)
		//	{
		//		collisionInfo.Side = TOP;
		//	}

		//	//Collides Top Side
		//	if( TopSide.fX != 0 && TopSide.fY != 0)
		//	{
		//		collisionInfo.Side = BOTTOM;
		//	}

		//	//Vector to Rect2 from Rect1
		//	collisionInfo.toCollider = Rect2Center - Rect1Center;
		//}

		//Return if collision occured
	}
	return Intersecting;
}

//BoxToCircleCollision
// -- Return if a game object collides with a cirle or cirular game object
bool Physics::BoxToCircle(CircleCollisionInfo collisionInfo, RECT rect, RECT circle)
{
	//Width
	int CircleWidth = circle.right - circle.left;
	int RectWidth = rect.right - rect.left;

	//Height
	int CircleHeight = circle.bottom - circle.top;
	int RectHeight = rect.bottom - rect.top;

	//Centers
	tVector2D CircleCenter;
	tVector2D RectCenter;

	//Calc Centers
	CircleCenter.fX = (float)CircleWidth / 2 + circle.left;
	CircleCenter.fY = (float)CircleHeight / 2 + circle.top;

	RectCenter.fX = (float)RectWidth / 2 + rect.left;
	RectCenter.fY = (float)RectHeight / 2 + rect.top;

	//Find Radius 
	int CircleRadius = CircleWidth / 2;

	//Find rect dist from center to corner
	tVector2D RectCorner;
	RectCorner.fX = (float)rect.right;
	RectCorner.fY = (float)rect.bottom;

	//Solve distance vec
	tVector2D DistanceVector = RectCorner - RectCenter;

	//Norm of distance vec
	int RectMaxColRange = (int)Vector2DLength(DistanceVector);

	//Solve for the distance between centers
	tVector2D DistanceBetweenCenterVec = CircleCenter - RectCenter;

	//Norm of the distance between center vec
	int DistanceBetweenCenters = (int)Vector2DLength(DistanceBetweenCenterVec);

	//Check if collision occured
	if ( DistanceBetweenCenters < RectMaxColRange + CircleRadius )
	{
		//Collision Occured
		collisionInfo.DistanceOverLapped = (RectMaxColRange + CircleRadius) - DistanceBetweenCenters;
		collisionInfo.side = (RectCenter.fX > CircleCenter.fX) ? C_RIGHT : C_LEFT;
		collisionInfo.VectorTo = Vector2DNormalize(DistanceBetweenCenterVec);

		return true;
	}
	return false;
}

//LineCast 
// -- Checks to see if any ojects collide with a line. If so returns the closest.
bool Physics::LineCast(LinecastInfo &linecastInfo, float distance, tVector2D StartingPoint, tVector2D direction, const char* ignoreID)
{
	//Current Closest Object Info
	GameObject * ClosestObject = nullptr;
	float ClosestObjectDistance = 999999999.0f;

	//Clear the lincastInfo just in case it was passed with values. 
	linecastInfo.Distance = 0.0f;
	linecastInfo.HitObject = nullptr;
	linecastInfo.side = CollisionSide::NONE;

	//Calculate the end point of the line by the starting point and the distance. 
	tVector2D EndPoint;
	EndPoint = StartingPoint + (direction * distance);


	//Loop through the object manager getting the 4 lines of the collision box. 
	for(int i = 1; ; i++)
	{
		//Skip Certain buckets
		if( i == 3)
			continue;

		const list<GameObject*>* CurrentBucket = ObjectManager::GetInstance()->GetBucket(i);

		if ( CurrentBucket == nullptr )
			break;

		//Loop through the bucket
		for( decltype(CurrentBucket->begin()) curElement = CurrentBucket->begin(); curElement != CurrentBucket->end(); curElement++ )
		{
			// Skip the object if the tag matches the ignored tag.
			if((*curElement)->GetObjectID() == ignoreID)
				continue;

			// Skip the object if the object is inactive.
			if((*curElement)->GetIsActive() == false)
				continue;

			RECT CurrColRect = (*curElement)->GetHitBox();

			//CornerPoints
			tVector2D UpperLeftCorner;
			tVector2D UpperRightCorner;
			tVector2D BottomLeftCorner;
			tVector2D BottomRightCorner;

			//Get Corners
			UpperLeftCorner.fX = (float)CurrColRect.left;
			UpperLeftCorner.fY = (float)CurrColRect.top;

			UpperRightCorner.fX = (float)CurrColRect.right;
			UpperRightCorner.fY = (float)CurrColRect.top;

			BottomLeftCorner.fX = (float)CurrColRect.left;
			BottomLeftCorner.fY = (float)CurrColRect.bottom;

			BottomRightCorner.fX = (float)CurrColRect.right;
			BottomRightCorner.fY = (float)CurrColRect.bottom;

			tVector2D LeftColPoint;
			tVector2D RightColPoint;
			tVector2D TopColPoint;
			tVector2D BottomColPoint;

			//Init 0
			LeftColPoint.fX		= LeftColPoint.fY	= 0;
			RightColPoint.fX	= RightColPoint.fY	= 0;
			TopColPoint.fX		= TopColPoint.fY	= 0;
			BottomColPoint.fX	= BottomColPoint.fY	= 0;

			// Store whether or not the lines collided.
			bool didCollideLeft		= false;
			bool didCollideRight	= false;
			bool didCollideTop		= false;
			bool didCollideBottom	= false;

			//Check LeftLine
			LeftColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, BottomLeftCorner, &didCollideLeft);
			RightColPoint = LineCollision(StartingPoint, EndPoint, UpperRightCorner, BottomRightCorner, &didCollideRight);
			TopColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, UpperRightCorner, &didCollideTop);
			BottomColPoint = LineCollision(StartingPoint, EndPoint, BottomLeftCorner, BottomRightCorner, &didCollideBottom);

			//////////////////////////////////////////////////
			// Find the closest point:

			if(didCollideLeft == true)
			{
				float dist = Distance(StartingPoint, LeftColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::LEFT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = LeftColPoint;
				}
			}
			if(didCollideRight == true)
			{
				float dist = Distance(StartingPoint, RightColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::RIGHT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = RightColPoint;
				}
			}
			if(didCollideTop == true)
			{
				float dist = Distance(StartingPoint, TopColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::TOP;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = TopColPoint;
				}
			}
			if(didCollideBottom == true)
			{
				float dist = Distance(StartingPoint, BottomColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::BOTTOM;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = BottomColPoint;
				}
			}

			// Store the shortest distance in the linecast info structure.
			linecastInfo.Distance = ClosestObjectDistance;
		}
	}

	return !(linecastInfo.HitObject == nullptr);

	/*
	//If no collision found
	tVector2D* ColPoint1 = nullptr;
	tVector2D* ColPoint2 = nullptr;

	//Store Colliding Points
	if( LeftColPoint.fX != 0 && LeftColPoint.fY != 0 )
	{
	ColPoint1 = &LeftColPoint;
	}

	if( RightColPoint.fX != 0 && RightColPoint.fY != 0 )
	{
	if( ColPoint1 == nullptr )
	ColPoint1 = &RightColPoint;
	else
	ColPoint2 = &RightColPoint;
	}

	if( TopColPoint.fX != 0 && TopColPoint.fY != 0 )
	{
	if( ColPoint1 == nullptr )
	ColPoint1 = &TopColPoint;
	else
	ColPoint2 = &TopColPoint;
	}

	if( BottomColPoint.fX != 0 && BottomColPoint.fY != 0 )
	{
	if( ColPoint1 == nullptr )
	ColPoint1 = &BottomColPoint;
	else
	ColPoint2 = &BottomColPoint;
	}
	//Check to see if there were collisions found
	if( ColPoint1 == nullptr )
	continue;

	//Calculate the distance
	int Distance = 0;
	if( ColPoint1 != nullptr && ColPoint2 != nullptr )
	{
	//Ints to store the distance to compare
	int Comp1Dist = 0;
	int Comp2Dist = 0;

	//Get the difference between col points and start point
	tVector2D Distance1Vec = (*ColPoint1 - StartingPoint);
	tVector2D Distance2Vec = (*ColPoint2 - StartingPoint);

	//Get the int distance value
	Comp1Dist = (int)Vector2DLength(Distance1Vec);
	Comp2Dist = (int)Vector2DLength(Distance2Vec);

	//Store the closest col point
	if( Comp1Dist > Comp2Dist )
	Distance = Comp2Dist;
	else
	Distance = Comp1Dist;
	}
	else
	{
	if(ColPoint1 != nullptr)
	{
	//Only one point get the distance
	Distance = (int)Vector2DLength(*ColPoint1);
	}
	}

	if( ClosestObject == nullptr )
	{
	//if not other object have been found assign this one
	ClosestObject = (*curElement);
	ClosestObjectDistance = (float)Distance;
	}
	else if( Distance < ClosestObjectDistance )
	{
	//compare to see if this object is closer than prev
	ClosestObjectDistance = (float)Distance;
	ClosestObject = (*curElement);
	}
	}
	}

	//See if an object was found, fill out line info, return
	if( ClosestObject != nullptr)
	{
	linecastInfo.Distance = (float)ClosestObjectDistance;
	linecastInfo.HitObject = ClosestObject;
	return true;
	}

	return false;
	*/
}

//Ignores a specific gameobject's memory address
bool Physics::LineCast(LinecastInfo &linecastInfo, float distance, tVector2D StartingPoint,
				tVector2D direction, GameObject* ignoreObj)
{
	//Current Closest Object Info
	GameObject * ClosestObject = nullptr;
	float ClosestObjectDistance = 999999999.0f;

	//Clear the lincastInfo just in case it was passed with values. 
	linecastInfo.Distance = 0.0f;
	linecastInfo.HitObject = nullptr;
	linecastInfo.side = CollisionSide::NONE;

	//Calculate the end point of the line by the starting point and the distance. 
	tVector2D EndPoint;
	EndPoint = StartingPoint + (direction * distance);


	//Loop through the object manager getting the 4 lines of the collision box. 
	for(int i = 1; ; i++)
	{
		//Skip Certain buckets
		if( i == 3)
			continue;

		const list<GameObject*>* CurrentBucket = ObjectManager::GetInstance()->GetBucket(i);

		if ( CurrentBucket == nullptr )
			break;

		//Loop through the bucket
		for( decltype(CurrentBucket->begin()) curElement = CurrentBucket->begin(); curElement != CurrentBucket->end(); curElement++ )
		{
			if((*curElement) == ignoreObj)
				continue;

			// Skip the object if the object is inactive.
			if((*curElement)->GetIsActive() == false)
				continue;

			RECT CurrColRect = (*curElement)->GetHitBox();

			//CornerPoints
			tVector2D UpperLeftCorner;
			tVector2D UpperRightCorner;
			tVector2D BottomLeftCorner;
			tVector2D BottomRightCorner;

			//Get Corners
			UpperLeftCorner.fX = (float)CurrColRect.left;
			UpperLeftCorner.fY = (float)CurrColRect.top;

			UpperRightCorner.fX = (float)CurrColRect.right;
			UpperRightCorner.fY = (float)CurrColRect.top;

			BottomLeftCorner.fX = (float)CurrColRect.left;
			BottomLeftCorner.fY = (float)CurrColRect.bottom;

			BottomRightCorner.fX = (float)CurrColRect.right;
			BottomRightCorner.fY = (float)CurrColRect.bottom;

			tVector2D LeftColPoint;
			tVector2D RightColPoint;
			tVector2D TopColPoint;
			tVector2D BottomColPoint;

			//Init 0
			LeftColPoint.fX		= LeftColPoint.fY	= 0;
			RightColPoint.fX	= RightColPoint.fY	= 0;
			TopColPoint.fX		= TopColPoint.fY	= 0;
			BottomColPoint.fX	= BottomColPoint.fY	= 0;

			// Store whether or not the lines collided.
			bool didCollideLeft		= false;
			bool didCollideRight	= false;
			bool didCollideTop		= false;
			bool didCollideBottom	= false;

			//Check LeftLine
			LeftColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, BottomLeftCorner, &didCollideLeft);
			RightColPoint = LineCollision(StartingPoint, EndPoint, UpperRightCorner, BottomRightCorner, &didCollideRight);
			TopColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, UpperRightCorner, &didCollideTop);
			BottomColPoint = LineCollision(StartingPoint, EndPoint, BottomLeftCorner, BottomRightCorner, &didCollideBottom);

			//////////////////////////////////////////////////
			// Find the closest point:

			if(didCollideLeft == true)
			{
				float dist = Distance(StartingPoint, LeftColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::LEFT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = LeftColPoint;
				}
			}
			if(didCollideRight == true)
			{
				float dist = Distance(StartingPoint, RightColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::RIGHT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = RightColPoint;
				}
			}
			if(didCollideTop == true)
			{
				float dist = Distance(StartingPoint, TopColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::TOP;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = TopColPoint;
				}
			}
			if(didCollideBottom == true)
			{
				float dist = Distance(StartingPoint, BottomColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::BOTTOM;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = BottomColPoint;
				}
			}

			// Store the shortest distance in the linecast info structure.
			linecastInfo.Distance = ClosestObjectDistance;
		}
	}

	return !(linecastInfo.HitObject == nullptr);
}


//LineCast 
// -- Checks to see if any ojects collide with a line, and returns the closest.
// -- This overload ignores objects with specified Memory addresses
bool Physics::LineCast(LinecastInfo &linecastInfo, float distance, tVector2D StartingPoint,
				tVector2D direction, vector<GameObject*>& ignoreVec)
{
	//Current Closest Object Info
	GameObject * ClosestObject = nullptr;
	float ClosestObjectDistance = 999999999.0f;

	//Clear the lincastInfo just in case it was passed with values. 
	linecastInfo.Distance = 0.0f;
	linecastInfo.HitObject = nullptr;
	linecastInfo.side = CollisionSide::NONE;

	//Calculate the end point of the line by the starting point and the distance. 
	tVector2D EndPoint;
	EndPoint = StartingPoint + (direction * distance);


	//Loop through the object manager getting the 4 lines of the collision box. 
	for(int i = 1; ; i++)
	{
		//Skip Certain buckets
		if( i == 3)
			continue;

		const list<GameObject*>* CurrentBucket = ObjectManager::GetInstance()->GetBucket(i);

		if ( CurrentBucket == nullptr )
			break;

		//Loop through the bucket
		for( decltype(CurrentBucket->begin()) curElement = CurrentBucket->begin(); curElement != CurrentBucket->end(); curElement++ )
		{
			bool skip = false;
			for(unsigned int ignoreIndex = 0; ignoreIndex < ignoreVec.size(); ++ignoreIndex)
			{
				if((*curElement) == ignoreVec[ignoreIndex])
				{
					skip = true;
					break;
				}
			}
			if(skip)
				continue;

			// Skip the object if the object is inactive.
			if((*curElement)->GetIsActive() == false)
				continue;

			RECT CurrColRect = (*curElement)->GetHitBox();

			//CornerPoints
			tVector2D UpperLeftCorner;
			tVector2D UpperRightCorner;
			tVector2D BottomLeftCorner;
			tVector2D BottomRightCorner;

			//Get Corners
			UpperLeftCorner.fX = (float)CurrColRect.left;
			UpperLeftCorner.fY = (float)CurrColRect.top;

			UpperRightCorner.fX = (float)CurrColRect.right;
			UpperRightCorner.fY = (float)CurrColRect.top;

			BottomLeftCorner.fX = (float)CurrColRect.left;
			BottomLeftCorner.fY = (float)CurrColRect.bottom;

			BottomRightCorner.fX = (float)CurrColRect.right;
			BottomRightCorner.fY = (float)CurrColRect.bottom;

			tVector2D LeftColPoint;
			tVector2D RightColPoint;
			tVector2D TopColPoint;
			tVector2D BottomColPoint;

			//Init 0
			LeftColPoint.fX		= LeftColPoint.fY	= 0;
			RightColPoint.fX	= RightColPoint.fY	= 0;
			TopColPoint.fX		= TopColPoint.fY	= 0;
			BottomColPoint.fX	= BottomColPoint.fY	= 0;

			// Store whether or not the lines collided.
			bool didCollideLeft		= false;
			bool didCollideRight	= false;
			bool didCollideTop		= false;
			bool didCollideBottom	= false;

			//Check LeftLine
			LeftColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, BottomLeftCorner, &didCollideLeft);
			RightColPoint = LineCollision(StartingPoint, EndPoint, UpperRightCorner, BottomRightCorner, &didCollideRight);
			TopColPoint = LineCollision(StartingPoint, EndPoint, UpperLeftCorner, UpperRightCorner, &didCollideTop);
			BottomColPoint = LineCollision(StartingPoint, EndPoint, BottomLeftCorner, BottomRightCorner, &didCollideBottom);

			//////////////////////////////////////////////////
			// Find the closest point:

			if(didCollideLeft == true)
			{
				float dist = Distance(StartingPoint, LeftColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::LEFT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = LeftColPoint;
				}
			}
			if(didCollideRight == true)
			{
				float dist = Distance(StartingPoint, RightColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::RIGHT;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = RightColPoint;
				}
			}
			if(didCollideTop == true)
			{
				float dist = Distance(StartingPoint, TopColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::TOP;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = TopColPoint;
				}
			}
			if(didCollideBottom == true)
			{
				float dist = Distance(StartingPoint, BottomColPoint);
				if(dist < ClosestObjectDistance)
				{
					ClosestObjectDistance = dist;
					linecastInfo.side = CollisionSide::BOTTOM;
					linecastInfo.HitObject = *curElement;
					linecastInfo.point = BottomColPoint;
				}
			}

			// Store the shortest distance in the linecast info structure.
			linecastInfo.Distance = ClosestObjectDistance;
		}
	}

	return !(linecastInfo.HitObject == nullptr);
}

//LineCollision
// -- Takes in four points making to lines and returning the point of collision if any
tVector2D Physics::LineCollision( tVector2D LineOneStart, tVector2D LineOneEnd, tVector2D LineTwoStart, tVector2D LineTwoEnd, bool* didCollide)
{
	//Point of intersection starting at 0 - 0
	tVector2D IntersectPoint;
	IntersectPoint.fX = 0;
	IntersectPoint.fY = 0;

	//Calculate the denominator
	//float Denominator = ((LineTwoEnd.fY - LineTwoStart.fY) * (LineOneEnd.fX - LineOneStart.fX)) - ((LineTwoEnd.fX - LineTwoStart.fX) * (LineOneEnd.fY - LineOneStart.fY));
	float Denominator = ((LineTwoEnd.fY - LineTwoStart.fY) * (LineOneEnd.fX - LineOneStart.fX)) - ((LineTwoEnd.fX - LineTwoStart.fX) * (LineOneEnd.fY - LineOneStart.fY));

	//Check to see if there is an intersection
	if( Denominator == 0)
	{
		if(didCollide != nullptr)
			*didCollide = false;
		return IntersectPoint;
	}
	else
	{
		//Needed for intersection point
		float CheckOne = (((LineTwoEnd.fX - LineTwoStart.fX) * (LineOneStart.fY - LineTwoStart.fY)) - ((LineTwoEnd.fY - LineTwoStart.fY) * (LineOneStart.fX - LineTwoStart.fX ))) / Denominator;

		//Needed to return 0 if intersection is past the point of the line segment. (Non infinite line)
		float CheckTwo = (((LineOneEnd.fX - LineOneStart.fX) * (LineOneStart.fY - LineTwoStart.fY)) - ((LineOneEnd.fY - LineOneStart.fY) * (LineOneStart.fX - LineTwoStart.fX))) / Denominator;

		//Return a Zero Vector if the point of collision is outside of the segments
		if( CheckOne < 0 || CheckOne > 1 || CheckTwo < 0 || CheckTwo > 1)
		{
			if(didCollide != nullptr)
				*didCollide = false;
			return IntersectPoint;
		}

		//Get the distance of the first line
		tVector2D LineOneDistance = LineOneEnd - LineOneStart;

		//Multiply distance by first check to get the distance between the starting point and the intersecting point
		LineOneDistance.fX *= CheckOne;
		LineOneDistance.fY *= CheckOne;

		//Calculate and return the intersecting point
		tVector2D IntersectingPointVec = LineOneStart + LineOneDistance;

		//Assign the Point to from the vector and return the point
		IntersectPoint.fX = IntersectingPointVec.fX;
		IntersectPoint.fY = IntersectingPointVec.fY;

		if(didCollide != nullptr)
			*didCollide = true;
		return IntersectPoint;
	}
}

//Update Collision Rect
// -- Updates the collision rect during the handle collision
/*static*/ RECT Physics::UpdateColRect(RECT& ColRect, int PosX, int PosY)
{
	int HorizontalDistance = PosX - ColRect.left;
	int VerticalDistance = PosY - ColRect.top;

	ColRect.left += HorizontalDistance;
	ColRect.right += HorizontalDistance;
	ColRect.top += VerticalDistance;
	ColRect.bottom += VerticalDistance;

	//RECT returnRect = ColRect;

	return ColRect;

}

// Distance
//	- Returns the distance between two points.
float Physics::Distance(const tVector2D& point1, const tVector2D& point2)
{
	float dx = point2.fX - point1.fX;
	float dy = point2.fY - point1.fY;

	return sqrtf((dx*dx) + (dy*dy));
}

// Parameters:
//		centerX		- The X position of the point to rotate around.
//		centerY		- The Y position of the point to rotate arount.
//		pointX			- The X position of the point to rotate.
//		pointY			- The Y position of the point to rotate.
//		angle			- The angle to rotate the point by.
// Return: In Parameters.
//		pointX			- The X position of the point after rotation.
//		pointY			- The Y position of the point after rotation.
void Physics::RotatePoint(float centerX, float centerY, float& pointX, float& pointY, float angle)
{
	float s = sinf(angle);
	float c = cosf(angle);
	float dx = pointX - centerX;
	float dy = pointY - centerY;
	float h = sqrtf(dx*dx + dy*dy);

	pointX -= centerX;
	pointY -= centerY;

	//float newX = pointX * c - pointY * s;
	//float newY = pointX * s + pointY * c;

	float newX = h * c;
	float newY = h * s;

	pointX = newX + centerX;
	pointY = newY + centerY;

	//pointX = cosf(angle) * (pointX - centerX) - sinf(angle) * (pointY - centerY) + centerX;
	//pointY = sinf(angle) * (pointX - centerX) + cosf(angle) * (pointY - centerY) + centerY;
}

// Parameters:
//		center		- The point to rotate around.
//		point		- The point to rotate.
//		angle		- The angle to rotate the point by.
// Return: In Parameters.
//		point		- The point after rotation.
void Physics::RotatePoint(tVector2D center, tVector2D& point, float angle)
{
	RotatePoint(center.fX, center.fY, point.fX, point.fX, angle);
}

// Author:
//	- Joshua Bramer
// Description:
//	- Calculate the center of a rectangle.
// Parameters:
//		box	- The box to calculate the center point of.
// Return: tVector2D
//		- The center point of the box.
tVector2D Physics::GetCenterPoint(const RECT& box)
{
	tVector2D center = 
	{
		(float)box.left + ((box.right - box.left) >> 1),
		(float)box.top + ((box.bottom - box.top) >> 1)
	};

	return center;
}

#pragma endregion

#pragma region LinecastInfo Methods

// Constructor
LinecastInfo::LinecastInfo(void)
{
	this->Distance = 0.0f;
	this->point.fX = 0.0f;
	this->point.fY = 0.0f;
	this->HitObject = nullptr;
	this->side = CollisionSide::NONE;
}

#pragma endregion